classdef AStarPlanner
    properties
        map
        resolution
        origin
    end
    
    methods
        function obj = AStarPlanner(map, resolution, origin)
            obj.map = map;
            obj.resolution = resolution;
            obj.origin = origin;
        end
        
        function path = plan(obj, start_world, goal_world)
            % 世界坐标转地图索引
            start_idx = obj.world_to_map(start_world);
            goal_idx = obj.world_to_map(goal_world);
            
            % 验证起点和终点
            if ~obj.is_valid_index(start_idx)
                error('起点(%.2f,%.2f)超出地图范围', start_world(1), start_world(2));
            end
            if ~obj.is_valid_index(goal_idx)
                error('终点(%.2f,%.2f)超出地图范围', goal_world(1), goal_world(2));
            end
            if obj.is_obstacle(start_idx)
                error('起点(%.2f,%.2f)位于障碍物上', start_world(1), start_world(2));
            end
            if obj.is_obstacle(goal_idx)
                error('终点(%.2f,%.2f)位于障碍物上', goal_world(1), goal_world(2));
            end

            % A*算法实现
            [rows, cols] = size(obj.map);
            open_set = containers.Map();
            closed_set = false(rows, cols);
            g_score = inf(rows, cols);
            f_score = inf(rows, cols);
            came_from = zeros(rows, cols, 2);
            
            % 初始化起点
            start_key = obj.index_to_key(start_idx);
            g_score(start_idx(1), start_idx(2)) = 0;
            f_score(start_idx(1), start_idx(2)) = obj.heuristic(start_idx, goal_idx);
            open_set(start_key) = [start_idx, f_score(start_idx(1), start_idx(2))];
            
            % 8邻域搜索
            directions = [ -1, 0; 1, 0; 0, -1; 0, 1; ...   % 上下左右
                          -1, -1; -1, 1; 1, -1; 1, 1]; % 对角线
            
            while ~isempty(open_set)
                % 获取f值最小的节点
                [current_idx, current_key] = obj.get_min_fscore(open_set);
                
                % 检查是否到达目标
                if isequal(current_idx, goal_idx)
                    path_idx = obj.reconstruct_path(came_from, current_idx);
                    path = obj.map_to_world(path_idx);
                    return;
                end
                
                % 移动到关闭列表
                remove(open_set, current_key);
                closed_set(current_idx(1), current_idx(2)) = true;
                
                % 检查所有邻居
                for k = 1:size(directions, 1)
                    neighbor_idx = current_idx + directions(k,:);
                    
                    % 跳过无效邻居
                    if ~obj.is_valid_index(neighbor_idx) || ...
                       obj.is_obstacle(neighbor_idx) || ...
                       closed_set(neighbor_idx(1), neighbor_idx(2))
                        continue;
                    end
                    
                    % 计算移动成本
                    if abs(directions(k,1)) == 1 && abs(directions(k,2)) == 1
                        move_cost = sqrt(2); % 对角线成本
                    else
                        move_cost = 1; % 水平/垂直成本
                    end
                    
                    tentative_g = g_score(current_idx(1), current_idx(2)) + move_cost;
                    
                    % 如果找到更优路径
                    if tentative_g < g_score(neighbor_idx(1), neighbor_idx(2))
                        came_from(neighbor_idx(1), neighbor_idx(2), :) = current_idx;
                        g_score(neighbor_idx(1), neighbor_idx(2)) = tentative_g;
                        f_score(neighbor_idx(1), neighbor_idx(2)) = tentative_g + ...
                            obj.heuristic(neighbor_idx, goal_idx);
                        
                        % 添加到开放列表
                        neighbor_key = obj.index_to_key(neighbor_idx);
                        if ~isKey(open_set, neighbor_key)
                            open_set(neighbor_key) = [neighbor_idx, f_score(neighbor_idx(1), neighbor_idx(2))];
                        end
                    end
                end
            end
            error('未找到可行路径');
        end
        
        function idx = world_to_map(obj, world_pos)
            % 世界坐标 → 地图索引
            x = world_pos(1) - obj.origin(1);
            y = world_pos(2) - obj.origin(2);
            col = floor(x / obj.resolution) + 1;
            row = floor(y / obj.resolution) + 1;
            idx = [row, col];
        end
        
        function world_pos = map_to_world(obj, path_idx)
            % 地图索引 → 世界坐标
            rows = path_idx(:,1);
            cols = path_idx(:,2);
            x = obj.origin(1) + (cols-1)*obj.resolution + obj.resolution/2;
            y = obj.origin(2) + (rows-1)*obj.resolution + obj.resolution/2;
            world_pos = [x, y];
        end
        
        function valid = is_valid_index(obj, idx)
            % 检查索引是否在地图范围内
            [rows, cols] = size(obj.map);
            valid = idx(1) >= 1 && idx(1) <= rows && ...
                    idx(2) >= 1 && idx(2) <= cols;
        end
        
        function obstacle = is_obstacle(obj, idx)
            % 检查是否为障碍物
            obstacle = obj.map(idx(1), idx(2)) == 1;
        end
        
        function h = heuristic(~, idx1, idx2)
            % 欧几里得距离启发式
            dx = idx1(2) - idx2(2);
            dy = idx1(1) - idx2(1);
            h = sqrt(dx^2 + dy^2);
        end
        
        function [min_idx, min_key] = get_min_fscore(obj, open_set)
            % 获取开放列表中f值最小的节点
            min_f = inf;
            min_key = '';
            min_idx = [0, 0];
            
            keys = open_set.keys;
            for i = 1:length(keys)
                key = keys{i};
                data = open_set(key);
                if data(3) < min_f
                    min_f = data(3);
                    min_key = key;
                    min_idx = data(1:2);
                end
            end
        end
        
        function path_idx = reconstruct_path(obj, came_from, current_idx)
            % 重建路径
            path_idx = current_idx;
            while true
                prev = squeeze(came_from(current_idx(1), current_idx(2), :))';
                if all(prev == [0, 0])
                    break;
                end
                path_idx = [prev; path_idx];
                current_idx = prev;
            end
        end
        
        function key = index_to_key(~, idx)
            % 将索引转换为唯一键
            key = sprintf('%d,%d', idx(1), idx(2));
        end
    end
end

